#include "SurfRansacMatcherI.h"
#include "Point.h"
#include <vector>

using namespace std;

SurfRansacMatcherI::SurfRansacMatcherI()
{
	name = "SurfRansacMatcherI";
	printf("new SurfRansacMatcherI\n");
	nr_iter = 500;
	distance_threshold = 0.001f;
	jc_threshold = 0.001f;
	giveup = 300;
	surf_theshold = 0.2;
}

SurfRansacMatcherI::SurfRansacMatcherI(int input_nr_iter, float input_distance_threshold, float input_jc_threshold, int input_giveup, float input_surf_theshold)
{
	nr_iter 			= input_nr_iter;
	distance_threshold 	= input_distance_threshold;
	jc_threshold 		= input_jc_threshold;
	giveup 				= input_giveup;
	surf_theshold 		= input_surf_theshold;
}

SurfRansacMatcherI::~SurfRansacMatcherI()
{
	printf("delete SurfRansacMatcherI\n");
}

void SurfRansacMatcherI::update(){
}

void SurfRansacMatcherI::print(){
	printf("name:%s\n",name.c_str());
	printf("params:\n");
	printf("nr_iter = %i\n",nr_iter);
	printf("distance_threshold = %f\n",distance_threshold);
	printf("jc_threshold = %f\n",jc_threshold);
	printf("giveup = %i\n",giveup);
	printf("surf_theshold = %f\n",surf_theshold);
}

const bool debugg_SurfRansacMatcherI = false;
Transformation * SurfRansacMatcherI::getTransformation(RGBDFrame * src, RGBDFrame * dst)
{
	if(debugg_SurfRansacMatcherI){printf("SurfRansacMatcherI::getTransformation(%i,%i)\n",src->id,dst->id);}
	IplImage* img_combine;
	int width;
	int height;
	if(debugg_SurfRansacMatcherI)
	{	
		width = src->width;
		height = src->height;
		
		img_combine = cvCreateImage(cvSize(2*width,height), IPL_DEPTH_32F, 3);
		float * data = (float *)img_combine->imageData;
		int index = 0;
		for (int j = 0; j < height; j++)
		{
			for (int i = 0; i < width; i++)
			{
				data[3 * (j * (2*width) + (width+i)) + 0] = float(dst->cloud->points[index].b)/255.0f;
				data[3 * (j * (2*width) + (width+i)) + 1] = float(dst->cloud->points[index].g)/255.0f;
				data[3 * (j * (2*width) + (width+i)) + 2] = float(dst->cloud->points[index].r)/255.0f;
				index++;
			}
		}
		index = 0;
		for (int j = 0; j < height; j++)
		{
			for (int i = 0; i < width; i++)
			{
				data[3 * (j * (2*width) + (i)) + 0] = float(src->cloud->points[index].b)/255.0f;
				data[3 * (j * (2*width) + (i)) + 1] = float(src->cloud->points[index].g)/255.0f;
				data[3 * (j * (2*width) + (i)) + 2] = float(src->cloud->points[index].r)/255.0f;
				index++;
			}
		}
		//cvReleaseImage( &img_a );
		//cvReleaseImage( &img_b );

	}
	
	Transformation * transformation = new Transformation();
	transformation->src = src;
	transformation->dst = dst;
	transformation->weight = 0;
	transformation->level = 1;

	//KeyPointSet * src_keypointset 	= src->keypoints;
	//KeyPointSet * dst_keypointset 	= dst->keypoints;
	vector<KeyPoint * > src_keypoints = src->keypoints->valid_key_points;
	vector<KeyPoint * > dst_keypoints = dst->keypoints->valid_key_points;

	int src_nr_points = src_keypoints.size();
	int dst_nr_points = dst_keypoints.size();
	
	if(debugg_SurfRansacMatcherI)
	{
		for(int i = 0; i < src_nr_points; i++){
			//cvCircle(img_combine,cvPoint(src_keypoints.at(i)->point->w		, src_keypoints.at(i)->point->h), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
		}
		for(int i = 0; i < dst_nr_points; i++){
			//cvCircle(img_combine,cvPoint(dst_keypoints.at(i)->point->w+width, dst_keypoints.at(i)->point->h), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
		}
	}

	float ** surf_distances = new float*[src_nr_points];
	for(int i = 0; i < src_nr_points;i++)
	{
		surf_distances[i] = new float[dst_nr_points];		
		for(int j = 0; j < dst_nr_points;j++)
		{
			FeatureDescriptor * descriptorA = src_keypoints.at(i)->descriptor;
			FeatureDescriptor * descriptorB = dst_keypoints.at(j)->descriptor;
			surf_distances[i][j] = descriptorA->distance(descriptorB);
		}
	}

	
	float * pos_src_x = new float[src_nr_points];
	float * pos_src_y = new float[src_nr_points];
	float * pos_src_z = new float[src_nr_points];
	
	float * pos_dst_x = new float[dst_nr_points];
	float * pos_dst_y = new float[dst_nr_points];
	float * pos_dst_z = new float[dst_nr_points];
	
	Eigen::Matrix4f transformationMat = Eigen::Matrix4f::Identity();
	
	for(int i = 0; i < src_nr_points;i++)
	{
		pos_src_x[i] = src_keypoints[i]->point->x;//position(0);
		pos_src_y[i] = src_keypoints[i]->point->y;
		pos_src_z[i] = src_keypoints[i]->point->z;;
		//if(debugg_SurfRansacMatcherI){cvCircle(img_combine,cvPoint(src_keypoints[i]->point->w, src_keypoints[i]->point->h), 5,cvScalar(0, 0, 255, 0),2, 8, 0);}
	}
	for(int i = 0; i < dst_nr_points;i++)
	{
		pos_dst_x[i] = dst_keypoints[i]->point->x;
		pos_dst_y[i] = dst_keypoints[i]->point->y;
		pos_dst_z[i] = dst_keypoints[i]->point->z;
		//if(debugg_SurfRansacMatcherI){cvCircle(img_combine,cvPoint(dst_keypoints[i]->point->w+width, dst_keypoints[i]->point->h), 5,cvScalar(0, 0, 255, 0),2, 8, 0);}
	}
	
	int * src_matches 				= new int[src_nr_points];
	int * dst_matches 				= new int[dst_nr_points];
	for(int i = 0; i < src_nr_points;i++)
	{
		src_matches[i] = -1;
		float best_value = 9999999;
		for(int j = 0; j < dst_nr_points;j++)
		{
			if(surf_distances[i][j]<best_value)
			{
				best_value = surf_distances[i][j];
				src_matches[i] = j;
			}
		}
		//ROS_INFO("BEST src MATCH: %i -> %i  = %f",i,src_matches[i],best_value);
	}
	for(int j = 0; j < dst_nr_points;j++)
	{
		dst_matches[j] = -1;
		float best_value = 9999999;
		for(int i = 0; i < src_nr_points;i++)
		{
			if(surf_distances[i][j]<best_value)
			{
				best_value = surf_distances[i][j];
				dst_matches[j] = i;
			}
		}
		//ROS_INFO("BEST dst MATCH: %i -> %i  = %f",j,dst_matches[j],best_value);
	}

	pcl::TransformationFromCorrespondences tfc;	
	KeyPoint ** src_match_id = new KeyPoint*[src_nr_points];
	KeyPoint ** dst_match_id = new KeyPoint*[src_nr_points];
	
	//int * src_matches_id = new int[src_nr_points];
	//int * dst_matches_id = new int[src_nr_points];
	int nr_matches = 0;
	
	for(int i = 0; i < src_nr_points;i++)
	{
		if(i == dst_matches[src_matches[i]] && surf_distances[i][src_matches[i]] < surf_theshold){
			//ROS_INFO("src MATCH: %i -> %i  = %f",i,src_matches[i],surf_distances[i][src_matches[i]]);
			
			KeyPoint * src_kp  = src_keypoints.at(i);
			KeyPoint * dst_kp  = dst_keypoints.at(src_matches[i]);
			src_match_id[nr_matches] = src_kp;
			dst_match_id[nr_matches] = dst_kp; 
			nr_matches++;
			if(debugg_SurfRansacMatcherI){
				//cvCircle(img_combine,cvPoint(dst_kp->point->w + width	, dst_kp->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
				//cvCircle(img_combine,cvPoint(src_kp->point->w			, src_kp->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
				//cvLine(img_combine,cvPoint(dst_kp->point->w  + width ,dst_kp->point->h),cvPoint(src_kp->point->w,src_kp->point->h),cvScalar(0, 0, 255, 0),1, 8, 0);
			}
		}
	}
	
	/*
	if(debugg_SurfRansacMatcherI){
		cvNamedWindow("combined image", CV_WINDOW_AUTOSIZE );
		cvShowImage("combined image", img_combine);
		cvWaitKey(0);
	}
	*/

	if(nr_matches >= 6)
	{
		KeyPoint * src_point;
		KeyPoint * dst_point;
		int best_fit 		= 0;
		
		int* best_indexes	= new int[nr_matches];
		bool ** jc = new bool*[nr_matches];
		for(int i = 0; i < nr_matches; i++){jc[i] = new bool[nr_matches];}
		
		for(int i = 0; i < nr_matches; i++)
		{
			jc[i][i] = false;
			for(int j = i+1; j < nr_matches; j++)
			{
				float dist_src = src_match_id[i]->point->distance(src_match_id[j]->point);
				float dist_dst = dst_match_id[i]->point->distance(dst_match_id[j]->point);
				//ROS_INFO("%i , %i ->dist_src = %f, dist_dst = %f, diff = %f",i,j,dist_src,dist_dst,fabs(dist_src-dist_dst));
				if(fabs(dist_src-dist_dst) < jc_threshold){
					jc[i][j] = true;
					jc[j][i] = true;
				}else{
					jc[i][j] = false;
					jc[j][i] = false;
				}
			}
		}
		
		for(int i = 0; i < nr_iter; i++)
		{
			//printf("iter: %i\n",i);
			
			int c = 0;
			const int c_limit = giveup;
			
			int match_a = rand() % nr_matches;
			int match_b = rand() % nr_matches;
			
			
			while(!jc[match_b][match_a]){
				match_b = rand() % nr_matches;
				if(c >= c_limit){break;}
				c++;
			}
			if(c >= c_limit){continue;}
			
			int match_c = rand() % nr_matches;
			while(!jc[match_c][match_b] || !jc[match_c][match_a]){
				match_c = rand() % nr_matches;
				if(c >= c_limit){break;}
				c++;
			}
			if(c >= c_limit){continue;}
			//ROS_INFO("C: %i",c);
			
			
			IplImage * img_combine_clone;
			if(debugg_SurfRansacMatcherI){
				img_combine_clone = cvCreateImage(cvSize(img_combine->width, img_combine->height), IPL_DEPTH_32F, 3);
				cvCopy( img_combine, img_combine_clone, NULL );
			
				cvCircle(img_combine_clone,cvPoint(src_match_id[match_a]->point->w		, src_match_id[match_a]->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
				cvCircle(img_combine_clone,cvPoint(dst_match_id[match_a]->point->w + width, dst_match_id[match_a]->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
				cvLine(img_combine_clone,cvPoint(src_match_id[match_a]->point->w ,src_match_id[match_a]->point->h),cvPoint(dst_match_id[match_a]->point->w + width,dst_match_id[match_a]->point->h),cvScalar(0, 0, 255, 0),1, 8, 0);
			
				cvCircle(img_combine_clone,cvPoint(src_match_id[match_b]->point->w		, src_match_id[match_b]->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
				cvCircle(img_combine_clone,cvPoint(dst_match_id[match_b]->point->w + width, dst_match_id[match_b]->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
				cvLine(img_combine_clone,cvPoint(src_match_id[match_b]->point->w ,src_match_id[match_b]->point->h),cvPoint(dst_match_id[match_b]->point->w + width,dst_match_id[match_b]->point->h),cvScalar(0, 0, 255, 0),1, 8, 0);
			
				cvCircle(img_combine_clone,cvPoint(src_match_id[match_c]->point->w		, src_match_id[match_c]->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
				cvCircle(img_combine_clone,cvPoint(dst_match_id[match_c]->point->w + width, dst_match_id[match_c]->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
				cvLine(img_combine_clone,cvPoint(src_match_id[match_c]->point->w ,src_match_id[match_c]->point->h),cvPoint(dst_match_id[match_c]->point->w + width,dst_match_id[match_c]->point->h),cvScalar(0, 0, 255, 0),1, 8, 0);
			}
			
			
			
			tfc.reset();
			tfc.add(src_match_id[match_a]->point->pos, dst_match_id[match_a]->point->pos);
			tfc.add(src_match_id[match_b]->point->pos, dst_match_id[match_b]->point->pos);
			tfc.add(src_match_id[match_c]->point->pos, dst_match_id[match_c]->point->pos);
			
			transformationMat = tfc.getTransformation().matrix();
			float mat00 = transformationMat(0,0);
			float mat01 = transformationMat(0,1);
			float mat02 = transformationMat(0,2);
			float mat03 = transformationMat(0,3);
			float mat10 = transformationMat(1,0);
			float mat11 = transformationMat(1,1);
			float mat12 = transformationMat(1,2);
			float mat13 = transformationMat(1,3);
			float mat20 = transformationMat(2,0);
			float mat21 = transformationMat(2,1);
			float mat22 = transformationMat(2,2);
			float mat23 = transformationMat(2,3);
			//float mat30 = transformationMat(3,0);
			//float mat31 = transformationMat(3,1);
			//float mat32 = transformationMat(3,2);
			//float mat33 = transformationMat(3,3);
			int nr_good_fitts = 0;

			for(int j = 0; j < nr_matches; j++)
			{
				src_point = src_match_id[j];
				dst_point = dst_match_id[j];
			
				float x = src_point->point->x;
				float y = src_point->point->y;
				float z = src_point->point->z;
				
				float dx = x*mat00+y*mat01+z*mat02+mat03-dst_point->point->x;
				float dy = x*mat10+y*mat11+z*mat12+mat13-dst_point->point->y;
				float dz = x*mat20+y*mat21+z*mat22+mat23-dst_point->point->z;

				if(dx*dx + dy*dy + dz*dz < distance_threshold){nr_good_fitts++;}
			}
			if(best_fit < nr_good_fitts)
			{
				best_fit = nr_good_fitts;
				nr_good_fitts = 0;
				for(int j = 0; j < nr_matches; j++)
				{
					src_point = src_match_id[j];
					dst_point = dst_match_id[j];
					
					float x = src_point->point->x;
					float y = src_point->point->y;
					float z = src_point->point->z;
				
					float dx = x*mat00+y*mat01+z*mat02+mat03-dst_point->point->x;
					float dy = x*mat10+y*mat11+z*mat12+mat13-dst_point->point->y;
					float dz = x*mat20+y*mat21+z*mat22+mat23-dst_point->point->z;
					if(dx*dx + dy*dy + dz*dz<distance_threshold){
						best_indexes[nr_good_fitts] = j;
						nr_good_fitts++;
						if(debugg_SurfRansacMatcherI){
							cvCircle(img_combine_clone,cvPoint(src_point->point->w		, src_point->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
							cvCircle(img_combine_clone,cvPoint(dst_point->point->w+width, dst_point->point->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
							cvLine(img_combine_clone,cvPoint(src_point->point->w		, src_point->point->h),cvPoint(dst_point->point->w+width, dst_point->point->h),cvScalar(0, 255, 0, 0),1, 8, 0);
						}
					}
				}
				if(debugg_SurfRansacMatcherI){
					printf("best_fit: %i\n",best_fit);
					cvNamedWindow("combined image clone", CV_WINDOW_AUTOSIZE );
					cvShowImage("combined image clone", img_combine_clone);
					cvWaitKey(0);
				}
			}
			if(debugg_SurfRansacMatcherI){cvReleaseImage( &img_combine_clone );}

		}
		
		
		tfc.reset();
		//transformation->matches_src = new KeyPoint*[best_fit];
		//transformation->matches_dst = new KeyPoint*[best_fit];
		//transformation->nr_matches 	= best_fit;
		for(int i = 0; i < best_fit; i++)
		{
			dst_point = dst_match_id[best_indexes[i]];
			src_point = src_match_id[best_indexes[i]];
			//transformation->matches_src[i] = src_point;
			//transformation->matches_dst[i] = dst_point;
			tfc.add(src_point->point->pos,dst_point->point->pos);
			if(debugg_SurfRansacMatcherI){
				printf("match surf distance: %f\n",dst_point->descriptor->distance(src_point->descriptor));
				cvLine(img_combine,cvPoint(src_point->point->w, src_point->point->h), cvPoint(dst_point->point->w+width, dst_point->point->h), cvScalar(0, 255, 0, 0), 1, 8, 0);
			}
		}
		transformation->transformationMatrix = tfc.getTransformation().matrix();
		if(debugg_SurfRansacMatcherI){printf("best_fit: %i\n",best_fit);}
		transformation->weight = float(best_fit)/2;
		
		for(int i = 0; i < nr_matches; i++){delete[] jc[i];}
		delete[] jc;
		delete[] best_indexes;
	}else{
		transformation->transformationMatrix = Eigen::Matrix4f::Identity();
	}

	if(debugg_SurfRansacMatcherI){
		cvNamedWindow("combined image", CV_WINDOW_AUTOSIZE );
		cvShowImage("combined image", img_combine);
		cvWaitKey(0);
		cvReleaseImage( &img_combine );
	}
	delete[] src_match_id;
	delete[] dst_match_id;
	delete[] src_matches;
	delete[] dst_matches;
	delete[] pos_src_x;
	delete[] pos_src_y;
	delete[] pos_src_z;
	delete[] pos_dst_x;
	delete[] pos_dst_y;
	delete[] pos_dst_z;

	for(int i = 0; i < src_nr_points;i++){delete[] surf_distances[i];}
	delete[] surf_distances;
	return transformation;
	//return NULL;
}
